#include <glog/logging.h>
#include "pangolin_viewer/viewer.h"
#include "read_data.h"

using namespace mapping;
using namespace pangolin_viewer;

static bool use_pangolin = true;
static std::string data_path = "/home/zl/work/data/";

int main(int argc, char** argv) {
    std::cout << std::fixed;         // 固定小数格式（避免科学计数法）
    std::cout << std::setprecision(10); // 保留6位小数（根据需求调整，如10位）
    std::cout << std::setw(10);      // 每个数值占15个字符宽度（避免挤压）
    std::cout << std::right;         // 右对齐（数值靠右，左侧补空格，便于列对齐）

    google::InitGoogleLogging(argv[0]);  // 初始化 glog
    FLAGS_logtostderr = true;           // 输出到 stderr（控制台）
    FLAGS_colorlogtostderr = true;  // 启用彩色输出

    std::shared_ptr<Publisher> publisher_ptr = std::make_shared<Publisher>();
    if(use_pangolin) {
        pangolin_viewer::viewer* viewer_ptr = new pangolin_viewer::viewer(publisher_ptr);
        std::thread* viewer_thread_ptr = new thread(&viewer::run, viewer_ptr);      
    }

    read_data read_file_data(data_path);
    Eigen::Matrix4d extrinsic_matrix = read_file_data.read_extrinsic();
    Eigen::Matrix3d intrinsic_matrix = read_file_data.read_intrinsic();
    
    std::vector<ImuData> imu_data_list;
    std::vector<WheelData> wheel_data_list;
    read_file_data.read_sensor_data(imu_data_list, wheel_data_list);

    std::cout << "读取到 " << imu_data_list.size() << " 条IMU数据:" << std::endl;
    std::cout << "读取到 " << wheel_data_list.size() << " 条Wheel数据:" << std::endl;

    LOG(INFO) << std::setprecision(15) << std::setw(10)<< "extrinsic_matrix : \n" << extrinsic_matrix << std::endl;

    SlidingWindow sliding_window(intrinsic_matrix, extrinsic_matrix, publisher_ptr);
    std::deque<float> velocity_queue;
    int frame_count = -1; 
    int failed_count = -1;
    WheelData last_wheel_data;
    while(1) {
        frame_count += 1;
        std::vector<FeaturePoint> feature_data;
        read_file_data.read_feature(feature_data);

        // std::cout << "读取到 " << frame_count << " feature : " << feature_data.size() << " 条Feature数据:" << std::endl;

        if(feature_data.empty()) {
            failed_count++;
            if(failed_count > 1) break;
            continue;
        }

        ImuData imu_data = imu_data_list[frame_count];
        WheelData wheel_data = wheel_data_list[frame_count];
        if(wheel_data.vehicle_speed < 1e-10) {
            if(frame_count > 0) {
                float dx = wheel_data.x - last_wheel_data.x;
                float dy = wheel_data.y - last_wheel_data.y;
                float dt = (wheel_data.timestamp - last_wheel_data.timestamp) / 1.0e6;
                float distance = std::sqrt(dx*dx + dy*dy);
                float velocity = distance / dt;
                
                // LOG(INFO) << " dx : " << dx << " , dy : " << dy << " , dt : " << dt << " , dist : " << distance << " , velocity : " << velocity << std::endl; 
                // LOG(INFO) << "velocity : " << velocity << std::endl; 

                if(velocity_queue.size() > 3) {
                    velocity_queue.pop_front();
                }
                velocity_queue.push_back(velocity);
                float mean_velocity = 0;
                for(const auto& vel : velocity_queue) {
                    mean_velocity += vel;
                }
                if(velocity_queue.size() > 0) {
                    mean_velocity /= velocity_queue.size();
                    velocity = mean_velocity;
                }

                // LOG(INFO) << "mean velocity : " << velocity << std::endl; 

                wheel_data.vehicle_speed = velocity;
            }
        }

        sliding_window.StartMapping(imu_data, wheel_data, feature_data);

        publisher_ptr->SetOdometryPose(Eigen::Vector3d(wheel_data.x, wheel_data.y, wheel_data.t));

        last_wheel_data = wheel_data;
        usleep(5000);
    }

    usleep(500000000);
    return 0;
}